#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <rclcpp/rclcpp.hpp>
#include <stdint.h>
// #include <serial_driver/serial_driver.hpp>
// #include <serial_driver/serial_port.hpp>
#include <serial/serial.h>
#include <customized_msgs/msg/can_motor_cmd.hpp>
#include <customized_msgs/msg/multi_can_motor_cmd.hpp>
#include <customized_msgs/msg/feed_back.hpp>
#include <queue>
// #include <socketcan_interface/socketcan.h>
#include <geometry_msgs/msg/point.hpp>
#include <std_msgs/msg/empty.hpp>
#include <chrono>
#include <customized_msgs/msg/topic_frequencies.hpp>
#include <customized_msgs/msg/winch_state.hpp>
#include "custom_filters/MeanFilter.hpp"
#include <customized_msgs/msg/ground_detection.hpp>
#include <gtest/gtest.h>
#include "std_msgs/msg/bool.hpp"

// #define SERIAL_DEBUG true
#define SERIAL_DEBUG false
// #define MASTER_SLAVE_SERIAL true //search for +2

#define CMD_MOTOR_ON 10
#define CMD_MOTOR_OFF 11
#define CMD_ZERO_POSITION 12
// #define CMD_MOTOR_RESET 13
#define CMD_MOTOR_CLEAR_ERRORS 14

#define CONTROL_MULTI_MOTOR 1

// #define MOTOR_FEEDBACK_ENABLE false
#define MOTOR_FEEDBACK_ENABLE true

// #define CONTROL_MODE 2  //  0 for p-v mode 1 for MIT mode 2 for E-MIT mode

#define CONTROLLER_TYPE 2 // 0 for daran , 1 for damiao , 2 for niren

#define ANSI_COLOR_RED "\x1b[31m"
#define ANSI_COLOR_GREEN "\x1b[32m"
#define ANSI_COLOR_YELLOW "\x1b[33m"
#define ANSI_COLOR_RESET "\x1b[0m"

struct Can_Motor_Cmd
{
    int id;
    double target_angle;
    double target_velocity;
    bool new_target = true;
    int control_mode = -1;
    double ref_torque = 0;

    // Can_Motor_Cmd(int _id, double _target_angle, double _target_velocity)

    // {
    //     id = _id;
    //     target_angle = _target_angle;
    //     target_velocity = _target_velocity;
    // }

    void init()
    {
        id = 0;
        target_angle = 0;
        target_velocity = 0;
        new_target = false;
    }
};

struct Motor_State
{
    int id;
    double est_angle;
    double est_velocity;
    double est_torque;

    void init()
    {
        id = 0;
        est_angle = 0;
        est_velocity = 0;
        est_torque = 0;
    }
};

class CanMotorDriver : public rclcpp::Node
{
public:
    int control_mode;
    serial::Serial ser;
    std::string serial_port_name;
    CanMotorDriver() : Node("can_motor_driver")
    {

        this->declare_parameter("serial_port_name", "/dev/ttyMotor");
        this->declare_parameter("control_frequency", 50.0);
        this->declare_parameter("torque_filter_time", 0.1);

        this->get_parameter("serial_port_name", serial_port_name);
        this->get_parameter("control_frequency", control_frequency);
        this->get_parameter("torque_filter_time", torque_filter_time);

        RCLCPP_WARN(this->get_logger(), "starting winch_can_motor_driver_node at %f hz  @%s port", control_frequency, serial_port_name.c_str());

        // Initialize subscriptions
        multi_write_sub = this->create_subscription<customized_msgs::msg::MultiCanMotorCmd>(
            "/winch_wireless_server/multi_can_motor_cmd", 10,
            std::bind(&CanMotorDriver::multi_can_motor_cmd_callback, this, std::placeholders::_1));

        write_sub = this->create_subscription<customized_msgs::msg::CanMotorCmd>(
            "/motor_control/motor_cmd", 10,
            std::bind(&CanMotorDriver::motor_cmd_callback, this, std::placeholders::_1));

        set_zero_sub = this->create_subscription<std_msgs::msg::Empty>(
            "/winch_wireless_server/set_zero", 10,
            std::bind(&CanMotorDriver::set_zero_callback, this, std::placeholders::_1));

        motor_reset_sub = this->create_subscription<std_msgs::msg::Empty>(
            "/winch_wireless_server/motor_reset", 10,
            std::bind(&CanMotorDriver::motor_reset_callback, this, std::placeholders::_1));

        // Initialize publishers
        serial_pub_motor_cmd_pub = this->create_publisher<customized_msgs::msg::CanMotorCmd>(
            "/winch_wireless_server/serial_pub_motor_cmd", 10);

        motor_fb_1_pub = this->create_publisher<customized_msgs::msg::FeedBack>(
            "/motor_feedback/motor1_winch", 10);

        motor_fb_2_pub = this->create_publisher<customized_msgs::msg::FeedBack>(
            "/motor_feedback/motor2_winch", 10);
        
        motor_switch_sub = this->create_subscription<std_msgs::msg::Bool>(
            "/winch_wireless_server/motor_switch", 10,std::bind(&CanMotorDriver::motor_switch_callback, this, std::placeholders::_1));

        // Initialize the timer
        timer = this->create_wall_timer(
            std::chrono::milliseconds(static_cast<int>(1000.0 / control_frequency)),
            std::bind(&CanMotorDriver::timerCallback, this));

        // Initialize command objects
        single_can_motor_cmd.init();
        for (int i = 0; i < 4; i++)
        {
            multi_can_motor_cmd[i].init();
        }

        // Example initialization values for multi_can_motor_cmd
        multi_can_motor_cmd[0].id = 3;
        multi_can_motor_cmd[0].control_mode = 0;
        multi_can_motor_cmd[0].target_angle = 0;
        multi_can_motor_cmd[0].target_velocity = 5;

        multi_can_motor_cmd[1].id = 4;
        multi_can_motor_cmd[1].control_mode = 1;
        multi_can_motor_cmd[1].ref_torque = -0.7;

        // Initialize motor states
        for (int i = 0; i < 2; i++)
        {
            motor_states[i].init();
        }

        // Prepare feedback messages
        customized_msgs::msg::FeedBack empty_msg;
        for (int i = 0; i < 2; i++)
        {
            fb_msg_vector.push_back(empty_msg);
        }

        // Initialize the torque filter
        // torque_filter = MeanFilter(torque_filter_time);

        // Set CAN packet length based on controller type
        if (CONTROLLER_TYPE == 2)
        {
            can_pack_length = 17;
        }
        else if (CONTROLLER_TYPE == 1)
        {
            can_pack_length = 16;
        }
    }

    void attemptReconnect()
    {
        while (true)
        {
            try
            {
                ser.close();
                ser.open();
                std::cout << "Serial port reconnected successfully." << std::endl;
                break; // 如果连接成功，则退出重连循环
            }
            catch (const std::exception &e)
            {
                std::cerr << "Failed to reconnect: " << e.what() << std::endl;
                std::this_thread::sleep_for(std::chrono::seconds(1)); // 等待一段时间后重试
            }

            if (!rclcpp::ok())
            {
                return;
            }
            
        }
    }
    void motor_switch_callback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "Received motor switch: %s", msg->data ? "True" : "False");
        if (msg->data==true){
           init_motors();
        }else{
            for (int i = 0; i < 4; i++)
            {
               motor_on_off_zero(i + 1, CMD_MOTOR_OFF);
            }
        }
    }
    void motor_reset_callback(const std_msgs::msg::Empty::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "resetting motors");

        for (int i = 0; i < 4; i++)
        {
            multi_can_motor_cmd[i].target_angle = 0;
            multi_can_motor_cmd[i].target_velocity = 0;
            multi_can_motor_cmd[i].new_target = true;
            multi_can_motor_cmd[i].ref_torque = 0;
        }
        run_control_loop();

        rclcpp::sleep_for(std::chrono::milliseconds(static_cast<int>(1000.0 / control_frequency)));
        motor_on_off_zero(3, CMD_MOTOR_CLEAR_ERRORS);
        rclcpp::sleep_for(std::chrono::milliseconds(100));
        motor_on_off_zero(3, CMD_MOTOR_ON);
        rclcpp::sleep_for(std::chrono::milliseconds(100));
        motor_on_off_zero(3, CMD_ZERO_POSITION);
    }

    bool in_white_list(int id)
    {
        for (int i = 0; i < white_list.size(); i++)
        {
            if (id == white_list[i])
                return true;
        }
        return false;
    }

    void set_zero_callback(const std_msgs::msg::Empty::SharedPtr msg)
    {
        // ROS_INFO("driver current time is %f",ros::Time::now().toSec());

        for (int i = 0; i < 4; i++)
        {
            multi_can_motor_cmd[i].target_angle = 0;
            multi_can_motor_cmd[i].target_velocity = 0;
            multi_can_motor_cmd[i].new_target = true;
            multi_can_motor_cmd[i].ref_torque = 0;
        }
        run_control_loop();

        rclcpp::sleep_for(std::chrono::milliseconds(static_cast<int>(1000.0 / control_frequency)));
        for (int i = 0; i < 4; i++)
        {
            motor_on_off_zero(i + 1, CMD_ZERO_POSITION);
        }
        RCLCPP_INFO(this->get_logger(), "zero set successfully");
    }

    void init_motors()
    {
        for (int i = 0; i < 4; i++)
        {
            motor_on_off_zero(i + 1, CMD_MOTOR_OFF);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
            motor_on_off_zero(i + 1, CMD_MOTOR_CLEAR_ERRORS);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
            motor_on_off_zero(i + 1, CMD_MOTOR_ON);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
            motor_on_off_zero(i + 1, CMD_ZERO_POSITION);
        }
    }

    void OpenSerial()
    {
        ser.setPort(serial_port_name);

        RCLCPP_INFO(this->get_logger(), "opening port: %s", serial_port_name.c_str());

        if (CONTROLLER_TYPE == 2)
            ser.setBaudrate(921600);
            // ser.setBaudrate(115200);

        else if (CONTROLLER_TYPE == 1)
            ser.setBaudrate(921600);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
        ClearSerialBuffer();
    }

    void update_send_data(uint8_t *buf, int id, int control_type)
    {
        if (control_type == 0) // position-velocity mode
        {
            send_data[14] = 0x01;
            send_data_2[1] = 0x01;
            // send_data_3[6] = 0x01;
            send_data_3[6] = 0x01;
        }
        else if (control_type == 2) // E-MIT mode
        {
            send_data[14] = 0x03;
            send_data_2[1] = 0x03;
            // send_data_3[6] = 0x03;
            send_data_3[6] = 0x03;
        }
        else if (control_type == 1) // MIT mode
        {
            send_data[14] = 0x00;
            send_data_2[1] = 0x00;
            // send_data_3[6] = 0x00;
            send_data_3[6] = 0x00;
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "control_type error");
            return;
        }

        send_data[13] = id;
        send_data_2[2] = id;
        // send_data_3[7] = id;
        send_data_3[7] = id;

        for (std::size_t i = 21, j = 1; i < 29; ++i, ++j)
        {
            send_data[i] = buf[j];
        }

        for (std::size_t i = 3, j = 1; i < 11; ++i, ++j)
        {
            send_data_2[i] = buf[j];
        }

        // for (std::size_t i = 8, j = 1; i < 16; ++i, ++j) {
        //     send_data_3[i] = buf[j];
        // }

        for (std::size_t i = 8, j = 1; i < 16; ++i, ++j)
        {
            send_data_3[i] = buf[j];
        }
    }

    void motor_cmd_callback(const customized_msgs::msg::CanMotorCmd::SharedPtr msg)
    {
        if (single_can_motor_cmd.target_angle == msg->target_angle && msg->id != single_can_motor_cmd.id)
        {
            return;
        }
        single_can_motor_cmd.id = msg->id;
        single_can_motor_cmd.target_angle = msg->target_angle;

        // 别管 id里没2就行，反正2的target_angle是反过来的
        if (msg->id == 2)
            single_can_motor_cmd.target_angle = -msg->target_angle;

        single_can_motor_cmd.target_velocity = msg->target_velocity;
        single_can_motor_cmd.new_target = true;
        single_can_motor_cmd.control_mode = msg->control_mode;
        single_can_motor_cmd.ref_torque = msg->ref_torque;

        // RCLCPP_INFO(this->get_logger(), "single cmd received id = %d target_angle = %f target_velocity = %f", single_can_motor_cmd.id, single_can_motor_cmd.target_angle, single_can_motor_cmd.target_velocity);
    }

    void multi_can_motor_cmd_callback(const customized_msgs::msg::MultiCanMotorCmd::SharedPtr msg)
    {
        // RCLCPP_INFO(this->get_logger(), "check1");
        for (int i = 0; i < msg->multi_can_motor_cmd.size(); i++)
        {
            multi_can_motor_cmd[i].id = msg->multi_can_motor_cmd[i].id;
            multi_can_motor_cmd[i].target_angle = msg->multi_can_motor_cmd[i].target_angle;

            if (msg->multi_can_motor_cmd[i].id == 2)
                multi_can_motor_cmd[i].target_angle = -multi_can_motor_cmd[i].target_angle;
            multi_can_motor_cmd[i].target_velocity = msg->multi_can_motor_cmd[i].target_velocity;

            if (msg->multi_can_motor_cmd[i].id == 2 && msg->multi_can_motor_cmd[i].control_mode == 1)
                multi_can_motor_cmd[i].target_velocity = -multi_can_motor_cmd[i].target_velocity;
            multi_can_motor_cmd[i].new_target = true;
            multi_can_motor_cmd[i].control_mode = msg->multi_can_motor_cmd[i].control_mode;
            multi_can_motor_cmd[i].ref_torque = msg->multi_can_motor_cmd[i].ref_torque;
            // RCLCPP_INFO(this->get_logger(), "id = %d target_angle = %d target_velocity = %d", multi_can_motor_cmd[i].id, multi_can_motor_cmd[i].target_angle, multi_can_motor_cmd[i].target_velocity);
        }
        // RCLCPP_INFO(this->get_logger(), "check2");
    }

    void printHex(const uint8_t *data, std::size_t size)
    {
        for (std::size_t i = 0; i < size; ++i)
        {
            std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(data[i]) << ' ';
        }
        std::cout << std::endl;
    }

    void printHex(const std::vector<uint8_t> &data)
    {
        for (const auto &byte : data)
        {
            std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << ' ';
        }
        std::cout << std::endl;
    }

    std::vector<uint8_t> queue2vector(std::queue<uint8_t> queue)
    {
        std::vector<uint8_t> vec;
        while (!queue.empty())
        {
            vec.push_back(queue.front());
            queue.pop();
        }

        return vec;
    }

    std::vector<uint8_t> queue2vector(std::queue<uint8_t> queue, int begin, int end)
    {
        int count = 0;
        std::vector<uint8_t> vec;
        while (!queue.empty())
        {
            if (count >= begin && count < end)
                vec.push_back(queue.front());
            count++;
            queue.pop();
        }
        return vec;
    }

    void wait_for_serial_data_callback(double time_budget, double least_time_budget = 0.001)
    {
        auto begin_time = this->now();
        while (true)
        {
            double elapsed_time = (this->now() - begin_time).seconds();
            if (elapsed_time > time_budget)
            {
                // RCLCPP_WARN(get_logger(), "wait_for_serial_data_callback timeout used %f ms", elapsed_time * 1000);
                break;
            }
            if (ser.available() && elapsed_time > least_time_budget)
            {
                break;
            }
        }
    }

    void write_serial()
    {
        // auto logger = this->get_logger();
        try
        {
            if (CONTROLLER_TYPE == 1)
            {
                ser.write(send_data, 30);
                if (SERIAL_DEBUG)
                {
                    RCLCPP_INFO(this->get_logger(), "sending:");
                    printHex(send_data, 30);
                }
            }
            else if (CONTROLLER_TYPE == 0)
            {
                ser.write(send_data_2, 11);
            }
            else if (CONTROLLER_TYPE == 2)
            {
                ser.write(send_data_3, 17);
                if (SERIAL_DEBUG)
                {
                    RCLCPP_INFO(this->get_logger(), "sending:");
                    printHex(send_data_3, 17);
                }
            }

            if (MOTOR_FEEDBACK_ENABLE)
            {
                wait_for_serial_data_callback(1.0 / control_frequency / 3.0);
                read_ttl_data_by_bite();
                rclcpp::sleep_for(std::chrono::milliseconds(static_cast<int64_t>(1000 / control_frequency / 20.0)));
            }
        }
        catch (const serial::SerialException &e)
        {
            // ROS_INFO("SerialException: %s", e.what());
            // ROS_INFO("ser.isOpen() = %d", ser.isOpen());
            RCLCPP_INFO(this->get_logger(), "SerialException: %s", e.what());
            RCLCPP_INFO(this->get_logger(), "ser.isOpen() = %d", ser.isOpen());
            attemptReconnect();
            // if (!ser.isOpen()) {
            //     // std::cerr << "Serial port disconnected. Attempting to reconnect..." << std::endl;
            //     ();
            // }
        }
        catch (const std::exception &e)
        {
            std::cerr << "Exception: " << e.what() << std::endl;
        }
    }

    void move_multi_motors()
    {
        for (int i = 0; i < 4; i++)
        {
            if (!in_white_list(multi_can_motor_cmd[i].id))
                continue;
            if (multi_can_motor_cmd[i].control_mode == 0)
                move_motor(multi_can_motor_cmd[i]);
            if (multi_can_motor_cmd[i].control_mode == 1)
                move_motor_MIT(multi_can_motor_cmd[i]);
            if (multi_can_motor_cmd[i].control_mode == 2)
                move_motor_E_MIT(multi_can_motor_cmd[i]);
        }
    }

    void move_motor(Can_Motor_Cmd &cmd)
    {
        // if(cmd.new_target == false)
        // {
        //     return;
        // }
        // cmd.new_target = false;

        int id = cmd.id;
        float target_angle = cmd.target_angle;
        float target_velocity = cmd.target_velocity;

        uint8_t *pbuf, *vbuf;
        pbuf = (uint8_t *)&target_angle;
        vbuf = (uint8_t *)&target_velocity;

        uint8_t buf[] = {id, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00};

        buf[1] = *pbuf;
        buf[2] = *(pbuf + 1);
        buf[3] = *(pbuf + 2);
        buf[4] = *(pbuf + 3);
        buf[5] = *vbuf;
        buf[6] = *(vbuf + 1);
        buf[7] = *(vbuf + 2);
        buf[8] = *(vbuf + 3);

        customized_msgs::msg::CanMotorCmd serial_pub_motor_cmd;
        serial_pub_motor_cmd.target_angle = target_angle;
        serial_pub_motor_cmd.target_velocity = target_velocity;
        serial_pub_motor_cmd_pub->publish(serial_pub_motor_cmd);

        update_send_data(buf, id, 0);
        write_serial();
    }

    int float_to_uint(float x, float x_min, float x_max, int bits)
    {
        float span = x_max - x_min;
        float offset = x_min;
        return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
    }

    void move_motor_E_MIT(Can_Motor_Cmd &cmd)
    {
        if (cmd.new_target == false)
        {
            return;
        }
        cmd.new_target = false;

        int id = cmd.id;
        float target_angle = cmd.target_angle;
        float target_velocity = cmd.target_velocity;

        uint8_t *pbuf, *vbuf;
        pbuf = (uint8_t *)&target_angle;

        uint8_t buf[] = {id, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00};

        // uint8_t buf[] = {id,0x00,0x00,0x40,0x40,0x00,0x00,0xa0,0x40};

        uint16_t v_scaled = static_cast<uint16_t>(target_velocity * 100); // 放大100倍并转换为无符号16位数据
        uint16_t torque_max_scaled = static_cast<uint16_t>(1.0 * 10000);  // 放大10000倍并转换为无符号16位数据

        // 将v_scaled的低8位和高8位分别赋值给buf的第四位和第五位

        buf[1] = *pbuf;
        buf[2] = *(pbuf + 1);
        buf[3] = *(pbuf + 2);
        buf[4] = *(pbuf + 3);
        buf[5] = static_cast<uint8_t>(v_scaled & 0xFF);
        buf[6] = static_cast<uint8_t>((v_scaled >> 8) & 0xFF);
        buf[7] = static_cast<uint8_t>(torque_max_scaled & 0xFF);
        buf[8] = static_cast<uint8_t>((torque_max_scaled >> 8) & 0xFF);

        update_send_data(buf, id, 2);
        write_serial();
    }

    void move_motor_MIT(Can_Motor_Cmd &cmd)
    {
        // // if (cmd.new_target == false || cmd.id > 5 || cmd.id <= 0)
        // if (cmd.new_target == false || cmd.id > 5 || cmd.id <= 0)

        // {
        //     return;
        // }
        cmd.new_target = false;

        int id = cmd.id;
        float target_angle = cmd.target_angle;
        float target_velocity = cmd.target_velocity;

        // double _KP = 40.0;
        // double _KD = 0.5;
        double _KP = 0.0;
        double _KD = 0.0;
        uint16_t pos_tmp, vel_tmp, kp_tmp, kd_tmp, tor_tmp;
        // double P_MIN = -12.5;
        // double P_MAX = 12.5;
        // double V_MIN = -30.0;
        // double V_MAX = 30.0;
        // double T_MIN = -10.0;
        // double T_MAX = 10.0;

        double P_MIN = -600;
        double P_MAX = 600;
        double V_MIN = -45;
        double V_MAX = 45;
        double T_MIN = -200;
        double T_MAX = 200;

        double KP_MIN = 0.0;
        double KP_MAX = 500.0;
        double KD_MIN = 0.0;
        double KD_MAX = 5.0;

        double _torq = 0;
        _torq = cmd.ref_torque;

        fb_msg_vector[id - 1].ref_torque = _torq;

        pos_tmp = float_to_uint(target_angle, P_MIN, P_MAX, 16);
        vel_tmp = float_to_uint(target_velocity, V_MIN, V_MAX, 12);
        kp_tmp = float_to_uint(_KP, KP_MIN, KP_MAX, 12);
        kd_tmp = float_to_uint(_KD, KD_MIN, KD_MAX, 12);
        tor_tmp = float_to_uint(_torq, T_MIN, T_MAX, 12);

        uint8_t *pbuf, *vbuf;
        pbuf = (uint8_t *)&target_angle;
        vbuf = (uint8_t *)&target_velocity;

        uint8_t buf[] = {id, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00};

        buf[1] = (pos_tmp >> 8);
        buf[2] = (pos_tmp);
        buf[3] = (vel_tmp >> 4);
        buf[4] = ((vel_tmp & 0xF) << 4) | (kp_tmp >> 8);
        buf[5] = kp_tmp;
        buf[6] = (kd_tmp >> 4);
        buf[7] = ((kd_tmp & 0xF) << 4) | (tor_tmp >> 8);
        buf[8] = tor_tmp;

        update_send_data(buf, id, 1);

        write_serial();
    }

    void motor_on_off_zero(int id, int cmd)
    {
        if (!in_white_list(id))
            return;

        // 0 for p-v  1 for MIT  2 for E-MIT
        int control_mode = 0;
        if (id == 3)
        {
            // control_mode = 2;
            control_mode = 0;
        }
        if (id == 4)
        {
            control_mode = 1;
        }

        uint8_t buf[] = {id, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00};

        if (cmd == CMD_MOTOR_ON)
            buf[8] = 0xFC;
        else if (cmd == CMD_MOTOR_OFF)
            buf[8] = 0xFD;
        else if (cmd == CMD_ZERO_POSITION)
            buf[8] = 0xFE;
        else if (cmd == CMD_MOTOR_CLEAR_ERRORS)
            buf[8] = 0xFB;
        else
            RCLCPP_WARN(rclcpp::get_logger("motor_control_node"), "invalid motor command!!");

        update_send_data(buf, id, control_mode);

        write_serial();

        RCLCPP_INFO(rclcpp::get_logger("motor_control_node"), "motor_on_off_zero id = %d cmd = %d", id, cmd);
    }

    int limit_angle(int input)
    {
        if (input > 1000)
            return 1000;
        else if (input < 0)
            return 0;
        else
            return input;
    }

    void run_control_loop()
    {

        if (CONTROL_MULTI_MOTOR)
        {
            move_multi_motors();
        }
        else
        {
            if (single_can_motor_cmd.control_mode == 0)
                move_motor(single_can_motor_cmd);
            if (single_can_motor_cmd.control_mode == 1)
                move_motor_MIT(single_can_motor_cmd);
            if (single_can_motor_cmd.control_mode == 2)
                move_motor_E_MIT(single_can_motor_cmd);
        }
    }

    // void timerCallback(const ros::TimerEvent &event)
    void timerCallback()
    {
        run_control_loop();
    }

    float uint_to_float(int x_int, float x_min, float x_max, int bits)
    {
        /// converts unsigned int to float, given range and number of bits ///
        float span = x_max - x_min;
        float offset = x_min;

        float output = (x_int - pow(2, bits - 1)) * span / (pow(2, bits) - 1);
        return output;
    }

    bool process_can_data(std::vector<uint8_t> recv_data_vector)
    {
        std::vector<uint8_t> can_data;
        if (CONTROLLER_TYPE == 1)
        {
            can_data = std::vector<uint8_t>(recv_data_vector.begin() + 7, recv_data_vector.begin() + 14);
        }
        else if (CONTROLLER_TYPE == 2)
        {
            can_data = std::vector<uint8_t>(recv_data_vector.begin() + 8, recv_data_vector.begin() + 15);
        }

        // RCLCPP_INFO(this->get_logger(), "decoding can_data is");
        // printHex(can_data);

        int id = can_data[0] & 0x0F;
        // if (id != 3)
        //     return false;

        int p_int = (can_data[1] << 8) | can_data[2];
        int v_int = can_data[3] << 4 | can_data[4] >> 4;
        int t_int = ((can_data[4] & 0x0F) << 8) | can_data[5];
        int mos_temperature = can_data[6];
        int rotor_temperature = can_data[7];

        double p_max = 600;

        if(id == 4)
            p_max = 600;

        double v_max = 45.0;
        double t_max = 54.0; // 8006

        float position = uint_to_float(p_int, -p_max, p_max, 16);
        float velocity = uint_to_float(v_int, -v_max, v_max, 12);
        float torque = uint_to_float(t_int, -t_max, t_max, 12);

        if (id == 3)
        {
            fb_msg_vector[0].id = id;
            fb_msg_vector[0].feedback_angle = position;
            fb_msg_vector[0].feedback_velocity = velocity;
            fb_msg_vector[0].feedback_torque = torque;
            fb_msg_vector[0].mos_temperature = mos_temperature;
            fb_msg_vector[0].rotor_temperature = rotor_temperature;
            fb_msg_vector[0].stamp = this->now();
            // torque_filter.add_data(torque);
            // fb_msg_vector[0].feedback_torque_filtered = torque_filter.get_mean();

            motor_fb_1_pub->publish(fb_msg_vector[0]);
        }

        if (id == 4)
        {
            fb_msg_vector[1].id = id;
            fb_msg_vector[1].feedback_angle = position;
            fb_msg_vector[1].feedback_velocity = velocity;
            fb_msg_vector[1].feedback_torque = torque;
            fb_msg_vector[1].mos_temperature = mos_temperature;
            fb_msg_vector[1].rotor_temperature = rotor_temperature;
            fb_msg_vector[1].stamp = this->now();
            // torque_filter.add_data(torque);
            // fb_msg_vector[0].feedback_torque_filtered = torque_filter.get_mean();

            motor_fb_2_pub->publish(fb_msg_vector[1]);
        }

        return true;
    }

    void clear_ttl_data_queue()
    {
        // ROS_INFO("clearing queue");
        while (!recv_can_data_queue.empty())
        {
            recv_can_data_queue.pop();
        }
    }

    void clear_ttl_data_queue(int begin, int end)
    {
        std::deque<int> temp_deque;

        while (!recv_can_data_queue.empty())
        {
            temp_deque.push_back(recv_can_data_queue.front());
            recv_can_data_queue.pop();
        }

        // ROS_INFO("temp_deque size = %d begin = %d end = %d",temp_deque.size(),begin,end);

        if (begin >= 0 && end < temp_deque.size() && begin <= end)
        {
            temp_deque.erase(temp_deque.begin() + begin, temp_deque.begin() + end + 1);
        }

        for (int elem : temp_deque)
        {
            recv_can_data_queue.push(elem);
        }
    }

    void check_ttl_data()
    {

        if (SERIAL_DEBUG)
        {
            RCLCPP_INFO(this->get_logger(), "checking ");
            printHex(queue2vector(recv_can_data_queue));
        }
        // ros::Duration(0.001).sleep();
        // RCLCPP_INFO(this->get_logger(), "recv_can_data_queue size = %d can_pack_length = %d", recv_can_data_queue.size(), can_pack_length);
        while (recv_can_data_queue.size() >= can_pack_length)
        {
            bool check_success_flag = false;
            // RCLCPP_INFO(this->get_logger(), "check1");
            for (size_t i = 0; i <= recv_can_data_queue.size() - can_pack_length; i++)
            {
                // RCLCPP_INFO(this->get_logger(), "check2 i = %d", i);
                std::vector<uint8_t> recv_data_vector = queue2vector(recv_can_data_queue, i, i + can_pack_length);
                // printHex(recv_data_vector);
                if (CONTROLLER_TYPE == 1 && recv_data_vector[0] == 0xAA && recv_data_vector[15] == 0x55 && recv_data_vector[1] == 0x11)
                {
                    process_can_data(recv_data_vector);
                    // clear_ttl_data_queue(i,i+can_pack_length-1);
                    clear_ttl_data_queue();
                    // ROS_INFO("check4");
                    break;
                }
                else if (CONTROLLER_TYPE == 2 && recv_data_vector[0] == 0xAA && recv_data_vector[16] == 0x7A )
                {
                    process_can_data(recv_data_vector);
                    clear_ttl_data_queue(i,i+can_pack_length-1);
                    check_success_flag = true;
                    // clear_ttl_data_queue();
                    break;
                }
            }

            if (check_success_flag == false)
                break;
        }
    }
    void ClearSerialBuffer()
    {
        RCLCPP_WARN(this->get_logger(), "data is blocked!! clear buffer");
        if (!ser.isOpen())
        {
            RCLCPP_ERROR(this->get_logger(), "Serial port is not open.");
            return;
        }
        int count = 0;
        // Clear input buffer
        while (ser.available() > 0)
        {
            std::vector<uint8_t> buffer(ser.available());
            ser.read(buffer.data(), buffer.size());
            count += buffer.size();
        }

        // You might also want to write a dummy byte and then flush the output buffer
        ser.flushOutput();
        RCLCPP_INFO(this->get_logger(), "Serial buffer of %d bytes has been cleared.", count);
    }

    void read_ttl_data_by_bite()
    {
        rclcpp::Time current_time = this->now();
        // ROS_INFO("recv_can_data_queue size = %d",recv_can_data_queue.size());
        uint8_t recv_data[100];
        int data_len = ser.available();

        // ROS_INFO("data_len = %d",data_len);
        int recv_len = 0;
        if (data_len > 0 && data_len <= 5 * can_pack_length)
            recv_len = ser.read(recv_data, data_len);
        // else if(data_len>5*can_pack_length)
        // {
        //     ClearSerialBuffer();
        //     return;
        // }
        // recv_len = serial_port_.read(recv_data,can_pack_length + gcs_pack_length);
        if (SERIAL_DEBUG)
        {
            RCLCPP_INFO(this->get_logger(), "recv_len = %d", recv_len);
            RCLCPP_WARN(this->get_logger(), "ttl data recv_data is:");
            printHex(recv_data, recv_len);
        }

        if (recv_len <= 0)
        {
            return;
        }
        rclcpp::Time end_time = this->now();
        // ROS_INFO("read %d bytes takes %f ms",recv_len,(end_time-current_time).toSec()*1000.0);
        for (int i = 0; i < recv_len; i++)
        {
            recv_can_data_queue.push(recv_data[i]);
        }
        // ROS_INFO("before poping size = %d",recv_can_data_queue.size());
        // if(recv_can_data_queue.size()>can_pack_length + gcs_pack_length)
        // {
        //     ROS_INFO("before poping ");
        //     printHex(queue2vector(recv_can_data_queue));
        // }
        // while(recv_can_data_queue.size()>can_pack_length)
        //     recv_can_data_queue.pop();
        check_ttl_data();

        end_time = this->now();
        if (SERIAL_DEBUG)
        {
            RCLCPP_INFO(this->get_logger(), "one loop takes %f ms", (end_time - current_time).seconds() * 1000.0);
        }
    }

private:
    double control_frequency;
    double torque_filter_time;
    int can_pack_length;
    // std::vector<int> white_list = {4};
    std::vector<int> white_list = {3,4};

    rclcpp::Subscription<customized_msgs::msg::MultiCanMotorCmd>::SharedPtr multi_write_sub;
    rclcpp::Subscription<customized_msgs::msg::CanMotorCmd>::SharedPtr write_sub;
    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr set_zero_sub;
    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr motor_reset_sub;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr motor_switch_sub;

    rclcpp::Publisher<customized_msgs::msg::CanMotorCmd>::SharedPtr serial_pub_motor_cmd_pub;
    rclcpp::Publisher<customized_msgs::msg::FeedBack>::SharedPtr motor_fb_1_pub;
    rclcpp::Publisher<customized_msgs::msg::FeedBack>::SharedPtr motor_fb_2_pub;

    rclcpp::TimerBase::SharedPtr timer;

    Can_Motor_Cmd multi_can_motor_cmd[4];
    Can_Motor_Cmd single_can_motor_cmd;
    Motor_State motor_states[2];
    MeanFilter torque_filter;
    std::vector<customized_msgs::msg::FeedBack> fb_msg_vector;
    std::queue<uint8_t> recv_can_data_queue;

    uint8_t send_data[30] = {0x55, 0xAA, 0x1e, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0a,
                             0x00, 0x00, 0x00, 0x00, 0, 0, 0, 0, 0x00, 0x08, 0x00, 0x00,
                             0, 0, 0, 0, 0, 0, 0, 0, 0x88};

    uint8_t send_data_2[11] = {0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    uint8_t send_data_3[17] = {0xAA, 0x00, 0x00, 0x08,
                               0x00, 0x00, 0x00, 0x00,
                               0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                               0x7A};
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CanMotorDriver>();

    try
    {
        node->OpenSerial();
    }
    catch (serial::IOException &e)
    {
        RCLCPP_ERROR(node->get_logger(), "Unable to open port: %s", e.what());
        return -1;
    }

    node->declare_parameter<int>("control_mode", 1);
    node->get_parameter("control_mode", node->control_mode);
    RCLCPP_INFO(node->get_logger(), "control_mode = %d", node->control_mode);

    if (node->ser.isOpen())
    {

        if (node->control_mode == 2)
        {
            for (int i = 0; i < 4; i++)
            {
                node->motor_on_off_zero(i + 1, CMD_ZERO_POSITION);
            }
            RCLCPP_INFO(node->get_logger(), "Motor zero point set");
            return 1;
        }
        else if (node->control_mode == 3)
        {
            for (int i = 0; i < 4; i++)
            {
                node->motor_on_off_zero(i + 1, CMD_MOTOR_OFF);
            }
            RCLCPP_INFO(node->get_logger(), "Motor turned off");
            return 1;
        }
        else if (node->control_mode == 4)
        {
            for (int i = 0; i < 4; i++)
            {
                node->motor_on_off_zero(i + 1, CMD_MOTOR_OFF);
                node->motor_on_off_zero(i + 1, CMD_MOTOR_CLEAR_ERRORS);
            }
            RCLCPP_INFO(node->get_logger(), "Motor errors cleared");
            return 1;
        }

        RCLCPP_INFO(node->get_logger(), "Serial Port initialized");

        node->init_motors();
        rclcpp::sleep_for(std::chrono::seconds(1));
        RCLCPP_INFO(node->get_logger(), "Motor initialization done");

        // if (!node->ser.isOpen())
        // {
        //     // ROS_INFO("Serial port disconnected. Attempting to reconnect...");
        //     RCLCPP_INFO(node->get_logger(), "Serial port disconnected. Attempting to reconnect...");
        //     // std::cerr << "Serial port disconnected. Attempting to reconnect..." << std::endl;
        //     node->attemptReconnect();
        // }

        rclcpp::spin(node);

        RCLCPP_INFO(node->get_logger(), "Quitting Can Motor Driver Node!! Shutting down motors");
        for (int i = 0; i < 4; i++)
        {
            node->motor_on_off_zero(i + 1, CMD_MOTOR_OFF);
        }

        rclcpp::shutdown();
        return 0;
    }
    else
    {
        return -1;
    }
}